#ifndef __VEHICLES_H
#define __VEHICLES_H

#include <vector>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <stdio.h>

class VehicleList
{
  public:
    std::vector<visualization_msgs::Marker> _vehs;

  private:
    std::string _frame_id;
    std::string _ns;
    ros::Time _stamp;
    uint32_t _shape;
    uint32_t _veh_num;
    uint32_t _base_idx;

  public:
    VehicleList(std::string frame_id = "/test_frame", std::string ns = "vehicles", ros::Time stamp = ros::Time::now(), uint32_t shape = visualization_msgs::Marker::ARROW);
    void AddVehicle(float pos_x, float pos_y, float pos_z, float size_x, float size_y, float size_z, float heading, float color_r = 0.0f, float color_g = 0.0f, float color_b = 1.0f);
    void VisualizerClear();
    uint32_t GetSize()
    {
        return this->_veh_num;
    }
    void ClearVehList()
    {
        this->_vehs.clear();
        this->_veh_num = 0;
    }
};

VehicleList::VehicleList(std::string frame_id, std::string ns, ros::Time stamp, uint32_t shape)
{
    this->_frame_id = frame_id;
    this->_stamp = stamp;
    this->_shape = shape;
    this->_veh_num = 0;
    this->_base_idx = 10;
    this->_ns = ns;
}

void VehicleList::AddVehicle(float pos_x, float pos_y, float pos_z, float size_x, float size_y, float size_z, float heading, float color_r, float color_g, float color_b)
{
    this->_veh_num++;
    visualization_msgs::Marker s_veh;
    s_veh.header.frame_id = this->_frame_id;
    s_veh.header.stamp = this->_stamp;
    s_veh.ns = this->_ns;
    s_veh.id = this->_veh_num + this->_base_idx;
    s_veh.type = this->_shape;
    s_veh.action = visualization_msgs::Marker::MODIFY;

    s_veh.pose.position.x = pos_x;
    s_veh.pose.position.y = pos_y;
    s_veh.pose.position.z = pos_z;

    float r_angle = heading * 3.1415 / 180.0;

    s_veh.pose.orientation.z = sin(r_angle / 2.0);
    s_veh.pose.orientation.w = cos(r_angle / 2.0);

    s_veh.scale.x = size_y * 0.8;
    s_veh.scale.y = size_x * 0.8;
    s_veh.scale.z = size_z * 0.8;

    s_veh.color.r = color_r;
    s_veh.color.g = color_g;
    s_veh.color.b = color_b;
    s_veh.color.a = 1.0;

    s_veh.lifetime = ros::Duration();

    this->_vehs.push_back(s_veh);
}

void VehicleList::VisualizerClear()
{

    this->_veh_num++;
    visualization_msgs::Marker deleter;
    deleter.action = visualization_msgs::Marker::DELETEALL;
    this->_vehs.push_back(deleter);
    
}

#endif